#include "EMath.h"
#include <math.h>
#include "eCPP.h"

namespace engine
{

//------------------------------------------------------------
const float EMath::f2PI     = 6.28318530717958647692528676655900577f;
const float EMath::fPI      = 3.14159265358979323846264338327950288f;
const float EMath::fPI_2    = 1.57079632679489661923132169163975144f;
const float EMath::fPI_3    = 1.0471975511965977461542144610932f;
const float EMath::fPI_4    = 0.78539816339744830961566084581988f;
const float EMath::fPI_5    = 0.6283185307179586476925286766559f;

//------------------------------------------------------------
void EMath::correctTrigoApproxErrors( float& fVal )
{
  // Handles approximation errors
  cASSERT( fVal >= -1.0001f );
  cASSERT( fVal <= 1.0001f );
  if( fVal < -1.f)
    fVal = -1.f;
  else if( fVal > 1.f )
    fVal = 1.f;
}

//------------------------------------------------------------
float EMath::moduloPI( const float fAngle0 )
{
  float fAngle = modulo2PI( fAngle0 );
  if( fAngle > fPI )
    fAngle -= f2PI;
  return fAngle;
}

//------------------------------------------------------------
float EMath::cos( const float fVal0 )
{
  float fVal = moduloPI( fVal0 );

  fVal += fPI_2;
  if( fVal >  fPI )
    fVal -= f2PI;

  if( fVal < 0.f )
  {
    float fCos = (1.27323954f + 0.405284735f * fVal ) * fVal;

    if (fCos < 0.f)
      return fCos * ( -0.225f * (fCos + 1.f ) + 1.f );
    else
      return fCos * ( 0.225f * (fCos - 1.f) + 1.f );
  }
  else
  {
    float fCos = (1.27323954f - 0.405284735f * fVal) * fVal;

    if (fCos < 0.f)
      return fCos * ( -0.225f * (fCos + 1.f ) + 1.f );
    else
      return fCos * ( 0.225f * (fCos - 1.f) + 1.f );
  }
}

//------------------------------------------------------------
float EMath::fastCos( const float fVal0 )
{
  float fVal = moduloPI( fVal0 );

  //compute cosine: sin(x + PI/2) = cos(x)
  fVal += fPI_2;
  if( fVal >  fPI )
    fVal -= f2PI;

  if( fVal < 0 )
    return (1.27323954f + 0.405284735f * fVal) * fVal;
  else
    return (1.27323954f - 0.405284735f * fVal) * fVal;
}

//------------------------------------------------------------
float EMath::sin( const float fVal0 )
{
  float fVal = moduloPI( fVal0 );

  if( fVal < 0.f )
  {
    float fSin = (1.27323954 + .405284735 * fVal) * fVal;

    if (fSin < 0.f)
      return fSin * ( -0.225f * (fSin + 1.f) + 1.f );
    else
      return fSin * ( 0.225f * (fSin - 1.f) + 1.f );
  }
  else
  {
    float fSin = (1.27323954 - 0.405284735 * fVal) * fVal;

    if (fSin < 0.f)
      return fSin * ( -0.225f * (fSin + 1.f) + 1.f );
    else
      return fSin * ( 0.225f * (fSin - 1.f) + 1.f );
  }
}

//------------------------------------------------------------
float EMath::fastSin( const float fVal0 )
{
  float fVal = moduloPI( fVal0 );

  if( fVal < 0 )
    return (1.27323954f + 0.405284735f * fVal) * fVal;
  else
    return (1.27323954f - 0.405284735f * fVal) * fVal;
}


//------------------------------------------------------------
float EMath::arcCos( float fVal )
{
  correctTrigoApproxErrors( fVal );

  return acos( fVal );
}

//------------------------------------------------------------
float EMath::fastArcCos( float fVal )
{
  correctTrigoApproxErrors( fVal );

  return acos( fVal );
}

//------------------------------------------------------------
float EMath::arcSin( float fVal )
{
  correctTrigoApproxErrors( fVal );

  return asin( fVal );
}

//------------------------------------------------------------
float EMath::fastArcSin( float fVal )
{
  correctTrigoApproxErrors( fVal );

  return asin( fVal );
}

//------------------------------------------------------------
float EMath::arcTan( const float fVal )
{
  //correctTrigoApproxErrors( fVal );

  return atan( fVal );
}

//------------------------------------------------------------
float EMath::fastArcTan( const float fVal )
{
  //correctTrigoApproxErrors( fVal );

  return atan( fVal );
}

//------------------------------------------------------------
float EMath::modulo2PI( const float fAngle )
{
  static const float fConst1 = f2PI / 65536;
  static const float fConst2 = 65536 / f2PI;
  return ( fConst1 * ( ( int )( fAngle	* fConst2 ) & 65535 ) );
}

//------------------------------------------------------------
float EMath::fastInvSqrt(const float fVal0 )
{
  float fHalfVal = 0.5f * fVal0;
  int i          = *(int*)&fVal0;            // get bits for floating value
  i              = 0x5f375a86 - (i>>1);     // gives initial guess y0
  float fVal     = *(float*)&i;             // convert bits back to float
  fVal           = fVal * ( 1.5f - fHalfVal * fVal * fVal ); // Newton step, repeating increases accuracy
  return fVal;
}

//------------------------------------------------------------
int EMath::toInt(const float fVal)
{
  int iVal;
  __asm fld fVal  
  __asm fistp iVal
  return iVal;
}

//------------------------------------------------------------
float EMath::min(const float fVal1, const float fVal2 )
{
  return fVal1 < fVal2 ? fVal1 : fVal2;
}

//------------------------------------------------------------
float EMath::min(const float fVal1, const float fVal2, const float fVal3 )
{
  float fMin = fVal1 < fVal2 ? fVal1 : fVal2;
  return fMin < fVal3 ? fMin : fVal3;
}

//------------------------------------------------------------
float EMath::max(const float fVal1, const float fVal2 )
{
  return fVal1 > fVal2 ? fVal1 : fVal2;
}

//------------------------------------------------------------
float EMath::max(const float fVal1, const float fVal2, const float fVal3 )
{
  float fMax = fVal1 > fVal2 ? fVal1 : fVal2;
  return fMax > fVal3 ? fMax : fVal3;
}

//------------------------------------------------------------
float EMath::abs( const float fVal )
{
  return ::abs( fVal );
}
/*

//------------------------------------------------------------
void EMath::clamp(float& fVal, float fMin, float fMax )
{
  fVal = fVal < fMin ? fMin : (fVal > fMax ? fMax : fVal );
}

//------------------------------------------------------------
void EMath::clampMin(float& fVal, float fMin )
{
  if( fVal < fMin )
    fVal = fMin;
}

//------------------------------------------------------------
void EMath::clampMax(float& fVal, float fMax )
{
  if( fVal > fMax )
    fVal = fMax;
}

//------------------------------------------------------------
void EMath::clamp(int& fVal, int fMin, int fMax )
{
  fVal = fVal < fMin ? fMin : (fVal > fMax ? fMax : fVal );
}

//------------------------------------------------------------
void EMath::clampMin(int& fVal, int fMin )
{
  if( fVal < fMin )
    fVal = fMin;
}

//------------------------------------------------------------
void EMath::clampMax(int& fVal, int fMax )
{
  if( fVal > fMax )
    fVal = fMax;
}*/

//------------------------------------------------------------
float EMath::fastApproxDist( const float fX, const float fY, const float fZ )
{
  const static float fConst1 = (1.f + 1.f / (4.f * (float)sqrt(3.))) * .5f;
  const static float fConst2 = ( 1.f / (float)sqrt(3.) );
  float fAbsX = abs( fX );
  float fAbsY = abs( fY );
  float fAbsZ = abs( fZ );
  return fConst1 * EMath::min( fConst2 * (fAbsX+fAbsY+fAbsZ) , EMath::max(fAbsX, fAbsY, fAbsZ) );
}

} // end namespace